/*
 * RangeFinder.java
 *
 * Created on May 31, 2008, 11:34 PM
 *
 * index:
 * 0 -> voltage reading, correlated to distance
 * 1 -> timestamp for reading
 */
package org.gunncs.actoriface;
import com.sun.spot.sensorboard.io.IScalarInput;
import java.io.*;

/**
 *
 * @author Anandg
 */
public class RangeFinder extends Sensor {
    public static final double SCALE = 10000;
    public static final double RANGE = 120.0; 
    public static final int HISTORY_SIZE = 7;
    private int pointer;
    private double[] history;
    private IScalarInput anPin;

    /** Creates a new instance of RangeFinder */
    public RangeFinder(IScalarInput an) {
        history = new double[HISTORY_SIZE];
        anPin = an;
        pointer = 0;
    }

    public synchronized void update(Thread daemon) {
        double[] d = new double[3];
        try {
            d[0] = (anPin.getValue());
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        d[0] = SCALE/d[0];
        history[pointer] = d[0];
        pointer++;
        pointer%=HISTORY_SIZE;
        d[1] = deviation(history);
        d[2] = (double) Simulator.time();
        double fil = average(history);
        d[0] = fil;
        setState(d);
    }
    
    public static double deviation(double[] history){
      double mean = average(history);
      int max = history.length-1;
      int min = 0;
      for(int i=0; i<history.length; i++){
        if(history[i]<=history[min]){
          min = i;
        }else if(history[i]>=history[max]){
          max = i;
        }
      }
      double sum =0;   
      for(int j=0; j<history.length; j++){
        if(j!= max && j!=min){
          sum +=((mean - history[j])*(mean - history[j]));
        }
      }
      sum = Math.sqrt(sum/(history.length-2));
      return sum;
    }
    
    public static double average(double[] history){
      int max = history.length-1;
      int min = 0;
      for(int i=0; i<history.length; i++){
        if(history[i]<=history[min]){
          min = i;
        }else if(history[i]>=history[max]){
          max = i;
        }
      }
      double sum = 0;
      for(int j=0; j<history.length; j++){
        if(j!= max && j!=min){
          sum += history[j];
        }
      }
      sum/=history.length-2;
      return sum;
    }

    public static double getDistance(double stateval) {
        return stateval * SCALE;
    }

    public String getName() {
        return "RangeFinder";
    }

    public int getID() {
        return 1;
    }
}
